import os
import time
import serial
from serial.tools import list_ports
from time import sleep
import threading
import tkinter as tk
from tkinter import messagebox
import asyncio
from bleak import BleakClient
import serial.tools.list_ports


imu_serial_port = serial.Serial()  # 创建串口
elbow_imu_serial_port = serial.Serial()  # 创建串口
acc_x = acc_y = acc_z = 0
gyro_x = gyro_y = gyro_z = 0
pitch = roll = yaw = 0
elbow_acc_x = elbow_acc_y = elbow_acc_z = elbow_gyro_x = elbow_gyro_y = elbow_gyro_z = elbow_pitch = elbow_roll = elbow_yaw = 0

IMU_MAC_ADDRESS = "F8:68:D4:2A:33:3A"

def scan_ble_devices():
    for port in serial.tools.list_ports.comports():
        if "USB" in port.description:  # 根据实际情况调整过滤条件
            return port.device
    raise Exception("未找到 IMU 串口")



def find_device_IMU_BWT61CL():
    timeout = 50
    while timeout != 0:
        port = None
        for p in list_ports.comports():
            if p[2].upper().startswith('USB VID:PID=1A86:7523'):
                port = p[0]
                print('find imu port: ' + port)
                print(p)
                return port

        sleep(0.1)
        timeout -= 1

    print('No imu found')
    return None

def find_device_MPU():
    timeout = 50
    while timeout != 0:
        port = None
        for p in list_ports.comports():
            if p[2].upper().startswith('USB VID:PID=0403:6001'):
                port = p[0]
                print('find mpu port: ' + port)
                return port

        sleep(0.1)
        timeout -= 1

    print('No mpu found')
    return None



def find_device():
    Com_Count = 0
    port = None
    port_list = []
    for p in list_ports.comports():
        port = p[0]
        # print('find port: ' + port)
        if port not in port_list:
            port_list.append(port)   
            Com_Count += 1   
    if port_list == '':      
        print('No COM found')
        return None
    else:
        return port_list, Com_Count  
    

Test_Dict = {'name' :'xiaoming',
             'age' : '18'}

def dict_test():
    print("name:%s"%Test_Dict.get('name'))

# ... existing code ...
def check_checksum(re_data):
    """
    检查数据包的校验和
    每11个字节一组，第11个字节是前10个字节的校验和
    """
    if len(re_data) < 11:
        return False
    
    # 计算有多少个完整的11字节组
    groups = len(re_data) // 11
    
    # 检查每一组
    for i in range(groups):
        start_idx = i * 11
        checksum_idx = start_idx + 10
        
        # 计算前10个字节的和
        calculated_sum = sum(re_data[start_idx:checksum_idx]) & 0xFF  # 取低8位
        
        # 比较与第11个字节
        if calculated_sum != re_data[checksum_idx]:
            print(f"校验和错误: 组 {i+1}, 计算值 {calculated_sum}, 接收值 {re_data[checksum_idx]}")
            return False
    
    return True

def sign_data_deal(re_data):
    if(re_data >= 0x8000):
        re_data -= 0x10000
    return re_data

def imu_data_deal(re_data):
    global acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,pitch,roll,yaw
    if len(re_data) >= 33:
        # 首先检查校验和
        if not check_checksum(re_data):
            print("腕部IMU数据校验失败，丢弃此帧")
            return
            
        if(re_data[0] == 0x55 and re_data[1] == 0x51):
            # 加速度输出
            acc_x = (sign_data_deal((re_data[3] << 8) | re_data[2])) / 32768 * 16 * 9.8
            acc_y = (sign_data_deal((re_data[5] << 8) | re_data[4])) / 32768 * 16 * 9.8
            acc_z = (sign_data_deal((re_data[7] << 8) | re_data[6])) / 32768 * 16 * 9.8
            
            # 校验和已经在check_checksum函数中验证，这里可以直接使用数据
            # print(acc_x, acc_y, acc_z)

            # 角速度输出
            if(re_data[11] == 0x55 and re_data[12] == 0x52):  # 注意索引调整为11和12
                gyro_x = sign_data_deal((re_data[14] << 8) | re_data[13]) / 32768 * 2000
                gyro_y = sign_data_deal((re_data[16] << 8) | re_data[15]) / 32768 * 2000
                gyro_z = sign_data_deal((re_data[18] << 8) | re_data[17]) / 32768 * 2000
                # print(gyro_x, gyro_y, gyro_z)

            # 角度输出
            if(re_data[22] == 0x55 and re_data[23] == 0x53):  # 注意索引调整为22和23
                pitch = sign_data_deal((re_data[25] << 8) | re_data[24]) / 32768 * 180
                roll = sign_data_deal((re_data[27] << 8) | re_data[26]) / 32768 * 180
                yaw = sign_data_deal((re_data[29] << 8) | re_data[28]) / 32768 * 180
                # print(pitch, roll, yaw)


def elbow_imu_data_deal(re_data):
    global elbow_acc_x, elbow_acc_y, elbow_acc_z, elbow_gyro_x, elbow_gyro_y, elbow_gyro_z, elbow_pitch, elbow_roll, elbow_yaw
    if len(re_data) >= 20:
        # 首先检查校验和 BWT901BLECL 5.0没有校验
        # if not check_checksum(re_data):
        #     print("肘部IMU数据校验失败，丢弃此帧")
        #     return

        # 加速度输出
        elbow_acc_x = (sign_data_deal((re_data[3] << 8) | re_data[2])) / 32768 * 16 * 9.8
        elbow_acc_y = (sign_data_deal((re_data[5] << 8) | re_data[4])) / 32768 * 16 * 9.8
        elbow_acc_z = (sign_data_deal((re_data[7] << 8) | re_data[6])) / 32768 * 16 * 9.8
        # print(elbow_acc_x, elbow_acc_y, elbow_acc_z)

        # 角速度输出
        elbow_gyro_x = sign_data_deal((re_data[9] << 8) | re_data[8]) / 32768 * 2000
        elbow_gyro_y = sign_data_deal((re_data[11] << 8) | re_data[10]) / 32768 * 2000
        elbow_gyro_z = sign_data_deal((re_data[13] << 8) | re_data[12]) / 32768 * 2000
        # print(elbow_gyro_x, elbow_gyro_y, elbow_gyro_z)

        # 角度输出
        elbow_pitch = sign_data_deal((re_data[15] << 8) | re_data[14]) / 32768 * 180
        elbow_roll = sign_data_deal((re_data[17] << 8) | re_data[16]) / 32768 * 180
        elbow_yaw = sign_data_deal((re_data[19] << 8) | re_data[18]) / 32768 * 180
        # print(elbow_pitch, elbow_roll, elbow_yaw)


##############################单片机串口线程函数#################################
from serial import SerialException

def imu_serial_rev_threading():
    print('IMU串口接收线程启动')
    buffer = bytearray()  # 使用缓冲区存储不完整的数据

    while imu_serial_port.isOpen():
        try:
            # 1. 非阻塞读取所有可用数据
            waiting = imu_serial_port.in_waiting
            if waiting > 0:
                buffer += imu_serial_port.read(waiting)

            # 2. 处理完整数据帧 (33字节)
            while len(buffer) >= 33:
                # 查找帧头0x55
                start_pos = -1
                for i in range(len(buffer) - 1):
                    if buffer[i] == 0x55 and (buffer[i + 1] in [0x51, 0x52, 0x53]):
                        start_pos = i
                        break

                # 没有找到有效帧头，丢弃无效数据
                if start_pos == -1:
                    buffer.clear()
                    break

                # 提取完整帧
                frame = buffer[start_pos:start_pos + 33]
                buffer = buffer[start_pos + 33:]  # 移除已处理数据

                # 处理数据帧
                imu_data_deal(frame)

            # 3. 控制读取频率 (匹配100Hz)
            time.sleep(0.003)  # 5ms延迟 (略小于10ms周期)

        except SerialException as e:
            print(f"串口错误: {e}")
            break
        except Exception as e:
            print(f"意外错误: {e}")
            buffer.clear()  # 清空缓冲区

    print('IMU串口接收线程退出')

def elbow_imu_serial_rev_threading():
    print('肘部IMU串口接收线程启动')
    buffer = bytearray()  # 使用缓冲区存储不完整的数据

    while elbow_imu_serial_port.isOpen():
        try:
            # 1. 非阻塞读取所有可用数据
            waiting = elbow_imu_serial_port.in_waiting
            if waiting > 0:
                buffer += elbow_imu_serial_port.read(waiting)

            # 2. 处理完整数据帧 (20字节)
            while len(buffer) >= 20:
                # 查找帧头0x55
                start_pos = -1
                for i in range(len(buffer) - 1):
                    if buffer[i] == 0x55 and buffer[i + 1] == 0x61:
                        start_pos = i
                        break

                # 没有找到有效帧头，丢弃无效数据
                if start_pos == -1:
                    buffer.clear()
                    break

                # 提取完整帧
                frame = buffer[start_pos:start_pos + 20]
                buffer = buffer[start_pos + 20:]  # 移除已处理数据

                # 处理数据帧
                elbow_imu_data_deal(frame)

            # 3. 控制读取频率 (匹配100Hz)
            time.sleep(0.003)  # 5ms延迟 (略小于10ms周期)

        except SerialException as e:
            print(f"串口错误: {e}")
            break
        except Exception as e:
            print(f"意外错误: {e}")
            buffer.clear()  # 清空缓冲区

    print('IMU串口接收线程退出')
def imu_serial_ope(port):
    # port = find_device_IMU_BWT61CL()
    imu_serial_port.port = port  # 列表获取串口号
    imu_serial_port.baudrate = 115200  # 列表获取波特率
    imu_serial_port.bytesize = 8  # 列表获取数据位数
    imu_serial_port.parity = 'N'  # crc_list.get() #列表获取奇偶效验
    imu_serial_port.stopbits = 1  # 列表获取停止位数
    imu_serial_port.timeout = 1
    imu_serial_port.open()  # 打开串口
    messagebox.showinfo("腕部IMU通信", "腕部IMU连接成功")  # 窗口提示
    if imu_serial_port.isOpen():
        messagebox.showinfo("腕部IMU通信", "打开腕部IMU串口成功")  # 窗口提示
        imu_serial_t = threading.Thread(target=imu_serial_rev_threading)
        imu_serial_t.Daemon = True
        imu_serial_t.start()  # 开始线程

def elbow_imu_serial_ope(port):
    # port = find_device_IMU_BWT61CL()
    elbow_imu_serial_port.port = port  # 列表获取串口号
    elbow_imu_serial_port.baudrate = 115200  # 列表获取波特率
    elbow_imu_serial_port.bytesize = 8  # 列表获取数据位数
    elbow_imu_serial_port.parity = 'N'  # crc_list.get() #列表获取奇偶效验
    elbow_imu_serial_port.stopbits = 1  # 列表获取停止位数
    elbow_imu_serial_port.timeout = 1
    elbow_imu_serial_port.open()  # 打开串口
    messagebox.showinfo("肘部IMU通信", "肘部IMU连接成功")  # 窗口提示
    if elbow_imu_serial_port.isOpen():
        messagebox.showinfo("肘部IMU通信", "打开肘部IMU串口成功")  # 窗口提示
        elbow_imu_serial_t = threading.Thread(target=elbow_imu_serial_rev_threading)
        elbow_imu_serial_t.Daemon = True
        elbow_imu_serial_t.start()  # 开始线程

def find_imu_ports():
    ports = serial.tools.list_ports.comports()
    imu1 = imu2 = None

    for port in ports:
        if "CH340" in port.description:
            if "1-2.2" in port.hwid:  # COM10 的物理位置  腕部传感器
                imu1 = port.device
            elif "1-2.3" in port.hwid:  # COM20 的物理位置  肘部传感器
                imu2 = port.device
        # print(port.hwid)

    if not imu1 and not imu2:
        raise Exception("未找到两个 IMU 设备")
    return imu1, imu2

def IMU_wrist_elbow_ope():
    try:
        imu1_port, imu2_port = find_imu_ports()
        print(f"IMU1 端口 腕部传感器: {imu1_port}（物理位置 1-2.2）")
        print(f"IMU2 端口 肘部传感器: {imu2_port}（物理位置 1-2.3）")
        if imu1_port:
            imu_serial_ope(imu1_port)
        if imu2_port:
            elbow_imu_serial_ope(imu2_port)
    except Exception as e:
        print(f"错误: {e}")


if __name__ == '__main__':
    IMU_wrist_elbow_ope()




